#include <Servo.h>

Servo gasServo;
Servo lenkServo;  // create servo object to control a servo
                // a maximum of eight servo objects can be created
 
int pos = 0;    // variable to store the servo position 
int count = 0;
int inByte;
int gas, minimum, lenkMax, lenkMin, gasMax, gasMin, incomingByte;
int gasSignalMin, gasSignalMax, lenkSignalMin, lenkSignalMax;

void setup() {
  count = 0;
  inByte = 0;
  lenkMax = 139;
  lenkMin = 30;
  gasMax = 125;
  gasMin = 75;
  gasSignalMin=20;
  gasSignalMax=45;
  lenkSignalMin=60;
  lenkSignalMax=85;
  lenkServo.attach(3);  // attaches the servo on pin 9 to the servo object 
  gasServo.attach(2);  // attaches the servo on pin 3 to the servo object 
  // initialize serial communication:
  Serial.begin(9600);
   // initialize the LED pins:
  for (int thisPin = 4; thisPin < 13; thisPin++) {
    pinMode(thisPin, OUTPUT);
  } 
  lenkServo.write(40);
  delay(500);
  setDefault();
  waitForBoot();
  //startUdpServer();
}

void loop() {
  // read the sensor:
  if (Serial.available() > 0) {
    int inByte = Serial.read();
    // do something different depending on the character received.  
    // The switch statement expects single number values for each case;
    // in this exmaple, though, you're using single quotes to tell
    // the controller to get the ASCII value for the character.  For
    // example 'a' = 97, 'b' = 98, and so forth:

    switch (inByte) {
    case 'c':    
      digitalWrite(4, HIGH);
      break;
    case 'd':    
      digitalWrite(5, HIGH);
      break;
    case 'e':    
      digitalWrite(6, HIGH);
      break;
     case 'g':    
      digitalWrite(13, HIGH);
      break;
    default:
      if(inByte <= gasSignalMax && inByte >= gasSignalMin){
        //inverting the signal for servo
        //inByte=gasSignalMin+(gasSignalMax-inByte);
        //mapping to propper serv output
        gas = map(inByte,gasSignalMin,gasSignalMax,gasMin,gasMax);
        gasServo.write(gas);
        count=0;
      }else if (inByte >= lenkSignalMin && inByte <= lenkSignalMax){
        //inverting
        //inByte=lenkSignalMin+(lenkSignalMax-inByte);
        //mapping
        pos = map(inByte,lenkSignalMin,lenkSignalMax,lenkMin,lenkMax);
        lenkServo.write(pos);
        count=0;
      } else {
        digitalWrite(13, HIGH);
      }
      // turn all the LEDs off:
      for (int thisPin = 4; thisPin < 13; thisPin++) {
        digitalWrite(thisPin, LOW);
      }
    }
  }
  count++;
  if(count>=1000){
    digitalWrite(13, HIGH);
    setDefault();
    delay(20);
    digitalWrite(13,LOW);
    count=0;
  }
  delay(1);
}

void setDefault(){
  lenkServo.write(90);
  gasServo.write(95);
}

void waitForBoot(){
  boolean booted=false;
  while (!booted){
    if (Serial.available() > 0) {
      int inByte = Serial.read();
      if(inByte == 'i')
      {
        digitalWrite(13, HIGH);
        delay(20);
        digitalWrite(13,LOW);
        inByte = Serial.read();
        //the keyword is "irq=3"
        if(inByte == 'r' && Serial.read() == 'q' && Serial.read() == '=' && Serial.read() == '3'){
            lenkServo.write(130);
            delay(500);
            setDefault();
            booted=true;
        }
      }
    }
  }
}

void startUdpServer(){
  boolean started = false;
  while(!started){
    if (Serial.available() > 0) {
      Serial.print('\n');
      if( Serial.read() == 'r' && Serial.read() == 'o' && Serial.read() == 'o') {
        Serial.print("/root/udpStarter.sh\n");
        started = true;
      }
      else {
        Serial.print('\n');
        delay(300);
        Serial.flush();
        Serial.print('\n');
      }
    }
  }
}
